#!/usr/bin/python
# -*- coding: UTF-8 -*-

import time,threading,sys
sys.path.append("/home/biowin/Minibot/04.RobotSamples/03.SpiderRobot")
# sys.path.append("../../")
from BwRobotLib import BwRobotLib
from SpiderRobot import SpiderRobot
from BwSensorLib import BwSensorLib
import os,inspect

# 加载机器人构型配置
fileName = inspect.getframeinfo(inspect.currentframe()).filename
folderPath     = os.path.dirname(os.path.abspath(fileName))
cfile=folderPath+"/config.txt"


class TransFaceRobot:
	def __init__(self):
		# 实例化传感器库对象
		self.sensorlib = BwsensorliblibLib("udp")
		# 实例化BwRobotLib库
		self.robotlib = BwRobotLib("udp")
		# 实例化蜘蛛机器人库
		self.spider = SpiderRobot(self.robotlib)
		# 连接机器人设备
		self.robotlib.connectRobot("10.10.100.254",8899) #真机AP模式
        # 导入机器人配置参数
        self.robotlib.loadConfig(config_file)
		#配置ID
		self.spider.bindIds(self.robotlib.module_ids)
		# 时间变量
		self.wait_time = 1
		# 点阵显示
		# 点阵图案：大眼　委屈　鄙视　无语　惊喜　左眨眼　右眨眼　开心　伤心 
		self.display_data = [[0x36,0x49,0x49,0x00,0x3E],[0x63,0x14,0x22,0x00,0x3E],[0x77,0x44,0x00,0x2A,0x14],
				[0x41,0x36,0x00,0x1C,0x22],[0x22,0x77,0x22,0x1C,0x14],[0x20,0x12,0x20,0x08,0x1C],
				[0x02,0x24,0x02,0x08,0x1C],[0x36,0x49,0x49,0x22,0x3E],[0x36,0x49,0x49,0x3E,0x22]]	
        # RGB彩灯参数
        # [255,1,1] 红色；[1,255,1] 绿色；[1,1,255] 蓝色；[255,255,1] 黄色; [255,1,255] 紫色 [1,255,255] 青色 [255,255,255] 白色； [100,100,100] 灰色; [100,255,100] 浅绿色
        self.rgb_value = [[255, 1, 1], [1, 255, 1], [1, 1, 255], [255,255,1],[255,1,255],[1,255,255]，[255,255,255],[100,100,100],[100,255,100]]


	# 点阵变脸,rgb灯变色
	def TransFaceRobot_Display(self):
			self.face_flag = 0 # 初始化图案序号
		while 1:
			dcm = self.sensorlib.readUltrasonicSensor() # 读取超声波传感器数据
			print("超声波返回数据：%.1fcm" % dcm)
			# 有障碍物在15-30cm
			if(dcm>15 and dcm<=30):
				self.sensorlib.setMatrixSensor(self.display_data[self.face_flag]) # 点阵变脸，显示不同表情
				self.sensorlib.setRGBLightSensor(self.display_data[self.face_flag]) # rgb灯亮不同颜色
				self.face_flag += 1
				if(self.face_flag==9):
					self.face_flag=0


	# 蜘蛛机器人前后踱步
	def transFaceRobotMotion(self):
			#---------创建线程对象----
		self.threading_1 = threading.Thread(target=self.TransFaceRobot_Display, args=())       # 实例化超声波传感器检测与点阵显示线程		
			#--------开启线程-----
		self.threading_1.start()
		# 蜘蛛机器人前后踱步
		while 1: 
			for a in range (0,2): # 向前走两步
				self.spider.goForward() 
			time.sleep(self.wait_time)
			for a in range (0,2): # 向后走两步
				self.spider.goBack()

# 主函数			
def main():
    # 实例化变脸机器人类
    transFaceRobot = TransFaceRobot()
    # 执行变脸机器人功能
    transFaceRobot.transFaceRobotMotion()


if __name__ == "__main__":
    try:
        main()
    except KeyboardInterrupt:
        print(" Program closed! ")
    pass
